1. Field of the Invention
The present invention relates to a position control device for a servomotor. More particularly, the present invention relates to a position control device for a servomotor in use of a NC machine tool, a robot or the like, which detects an unusual condition at a high speed so as to stop the servomotor in short time to minimize damage of the collision wherein the unusual condition is, for example, a case that a control shaft collides with a work piece in error during the operation.
2. Description of the Prior Art
FIG. 14 is a block diagram showing functions of a position control device for a servomotor. In FIG. 14, a position control unit 2 receives a rotational positioning command Pc for a servomotor 5 from a positioning command producing unit 1 and feeds back a motor rotational position signal Pf from a rotational position detector 7 of the servomotor 5 and then outputs a velocity command Vc to reduce position errors. A velocity control unit 3 feeds back a servomotor rotational velocity signal Vf from a velocity detecting unit 8 and outputs a current command Ic to a current control unit 4 to reduce velocity errors.
In the conventional embodiment mentioned above, the velocity detecting unit 8 calculates the rotational velocity signal Vf by differentiating the position signal received from the position detector disposed on the motor 5. For controlling the position, there is a case that a position of a machine is controlled by the position detector 7 such as a linear scale disposed on a machine, besides the case that a rotation angle position of a feed screw connected to an output shaft of the servomotor 5 such as the conventional embodiment.
A current control unit 4 controls the current to change a torque current of the servomotor to a torque current command Ic. The machine 6 may be a table, a robot arm or the like driven by the servomotor 5. The position control device as a whole controls the machine 6 corresponding to the command from the position command producing unit 1.
A disturbance torque observer 9 receives the torque current command Ic and the motor rotational velocity signal Vf and estimates a disturbance torque applied to the machine 6. A determination unit 10 determines whether the estimated disturbance torque signal TL* is over an allowable disturbance torque signal TLa or not. When the estimated disturbance torque signal TL* is over the allowable disturbance torque signal TLa, the determination: unit 10 outputs an alarm signal to the current control unit 4. At this point, the current control unit 4 stops current supply to the motor 5 and stops the servomotor 5.
FIG. 15 is a block diagram showing functions of one structural embodiment of the disturbance torque observer 9. In FIG. 15, the numerals 91 and 93 designate multiplication means and for multiplying a motor torque constant KT, and a total J of an inertia of the servomotor and a load inertia of a motor shaft conversion, respectively. The numeral 92 is subtraction means, The numeral 94 is differential means, and The numeral 95 is a primary filter unit of a time constant T.
A torque constant of the motor is designated by I, the motor torque constant is designated by KT, the total of the inertia of the servomotor and the moment of load inertia of the motor shaft conversion is designated by J, the rotational velocity of the servomotor is designated by Vf, and the disturbance torque is designated by TL. Then, an operational equation of the servomotor is as follows; EQU J.multidot.dVf/dt=KT.times.I+TL (1)
Then, the disturbance torque signal TL is estimated by the following equation which is a transformation of the equation (1). EQU TL=J.multidot.dVf/dt-KT.times.I (2)
The disturbance torque signal TL is estimated by the equation (2) with the current command Ic instead of the torque current I of the servomotor, thereby the block diagram of FIG. 15 stands up. In FIG. 15, a torque signal Tr substantially applied to the motor is calculated by multiplying differential value of the rotational velocity of the servomotor by the inertia J. Thereby, the disturbance torque is calculated by subtracting a producing torque signal Tm of the motor which is multiplied the current command Ic by the motor torque constant KT. And then transitional estimated errors are eliminated through the primary filter 95 to output the estimated torque signal TL* is outputted.
As mentioned above, the disturbance torque observer 9 has a function of taking out an external force which a torque element produced from the servomotor by the torque current command Ic is excluded from the torque regarding to the servomotor 5, wherein the external force is the disturbance torque signal TL. For instance, in a case that a slide or cutting tool driven by the control shaft is collided with any obstruction during the operation of a machine tool cutting the work piece, the disturbance torque signal TL is applied to the servomotor in a direction of preventing rotation. In a case of usual cutting or the like, the disturbance torque signal TL, that is cutting load, is not so large. However, when the control shaft is collided with the work peace in error, a quite large disturbance torque is loaded to the servomotor, differently from in the case of the normal cutting.
Conventionally, the allowance disturbance torque signal TLa is set between a normal value and an abnormal value of the disturbance torque, when the estimated disturbance torque signal TL* is over the allowance disturbance torque signal TLa, the determining unit determines the collision and outputs the alarm signal to the current control unit 4. The current control unit 4, then, shuts out the current supply to the servomotor 5 to prevent the machine 6 from breaking. FIG. 16 shows a relationship between time responses of the estimated disturbance torque at a normal value and at collision and the allowance disturbance torque signal TLa. In FIG. 16, an axis of abscissa designates time, and an axis of ordinate designates torque. In FIG. 16, P designates a normal value, Q designates a time response of the estimated disturbance torque signal TL* at collision and outputs the alarm signal when the estimated disturbance torque signal TL* is over the allowance disturbance torque signal TLa.
As mentioned above, in the position control device for the conventional servomotor, the determining unit 10 determines the collision and shuts out the current supply to the servomotor to prevent the machine from breaking. However, in a case that the servomotor rotates at a high speed, the machine is not always reduced the damage.
That is, there are problems for determining the collision; it is necessary that the allowance disturbance torque signal TLa is set higher than normal; a determining time lag Td is caused between an actual collision and determination of the collision as shown in FIG. 16 by delaying the primary filter eliminating the estimated errors of the disturbance observer. If it is assumed that the rotational velocity of the servomotor is Vf, as the servomotor rotates by Vf.times.Td, in a case that the determining time lag Td is constant, a rotational volume of the servomotor after collision, that is a braking distance, is increased proportionally to the rotational velocity signal Vf and thereby the damage of the machine 6 is increased. Therefore, at a high speed rotation, there were some cases that the machine 6 was damaged.